#!/usr/bin/python3
import rospy
import tf2_ros
from geometry_msgs.msg import Twist

from nav_msgs.msg import Odometry

import math
import tf



def callback_actspeed(odom):
  global act_x_speed, act_y_speed, act_yaw_speed
  act_x_speed = 100 * odom.twist.twist.linear.x
  act_y_speed = 100 * odom.twist.twist.linear.y
  act_z_speed = 100 * odom.twist.twist.linear.z
  #rospy.loginfo("%d, %d", act_x_speed, act_y_speed)
  return


class RobotController:
    def __init__(self):
        # 常量定义
        self.MAX_LSPEED = 15
        self.MAX_ASPEED = 15
        self.MAX_INTEGRAL = 10
        
        # PID参数
        self.STRAIGHT_KP = 0.5
        self.STRAIGHT_KI = 0
        self.STRAIGHT_KD = 0
        self.YAW_KP = 0
        self.YAW_KI = 0
        self.YAW_KD = 0

        #最小误差
        self.d_xy = 4
        self.d_yaw = 3
        self.d_speed = 3
        
        # 目标和误差
        self.x_target = 0
        self.y_target = 0
        self.yaw_target = 0
        self.x_error = 0
        self.y_error = 0
        self.yaw_error = 0
        self.x_integral = 0#最小误差
        
        self.y_integral = 0
        self.x_last_error = 0
        self.y_last_error = 0
        self.yaw_last_error = 0

        
        #当前状态
        self.flag = 0
        # 速度消息
        self.vel_msg = Twist()
        
        # ROS初始化
        rospy.init_node('control', anonymous=True)
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.flag = 0
        # TF缓存
        self.tf_buffer = tf2_ros.Buffer()
        tf2_ros.TransformListener(self.tf_buffer)

        rospy.loginfo("Listening for TF data...")
        
    def move(self, Xtarget, Ytarget, Yawtarget):
        """设置目标位置和偏航角。"""
        self.x_target = Xtarget
        self.y_target = Ytarget
        self.yaw_target = Yawtarget

    def detect_target(self):
        global act_x_speed, act_y_speed, act_yaw_speed
        """检查是否到达目标点。"""
        return (abs(self.x_error) <= self.d_xy and 
        abs(self.y_error) <= self.d_xy and 
        abs(self.yaw_error) <= self.d_yaw and 
        abs(act_x_speed) <= self.d_speed and 
        abs(act_y_speed) <= self.d_speed)
    
    def callback(self, transform):
        """处理变换并控制机器人。"""
        # 提取位姿数
        translation = transform.transform.translation
        rotation = transform.transform.rotation

        # 四元数转换为欧拉角
        _, _, yaw = tf.transformations.euler_from_quaternion((rotation.x, rotation.y, rotation.z, rotation.w))
        yaw = yaw * 180 / math.pi

        # 获取当前坐标（单位：cm）
        x = translation.x * 100
        y = translation.y * 100


        #打印输出
        rospy.loginfo("x: %.3f, y: %.3f", x, y)
        rospy.loginfo("yaw: %.3f", yaw)
        rospy.loginfo("flag: %d, %d" , self.flag, self.detect_target())

        # 计算误差
        self.x_error = self.x_target - x
        self.y_error = self.y_target - y
        self.yaw_error = self.yaw_target - yaw


        # 检查是否到达目的地
        #前进
        if (self.flag == 0) and (self.detect_target()):
            self.move(100, 0, 0)
            self.flag = 1
        elif ((self.flag == 1) and (self.detect_target())):
            self.move(100, 100, 0)
            self.flag = 2
        elif ((self.flag == 2) and (self.detect_target())):
            self.move(0, 100, 0)
            self.flag = 3
        elif ((self.flag == 3) and (self.detect_target())):
            self.move(0, 0, 0)
            self.flag = 4
        
        
        
        # 计算积分值并限幅



        
        self.x_integral = min(max(self.x_integral + self.x_error, -self.MAX_INTEGRAL), self.MAX_INTEGRAL)
        self.y_integral = min(max(self.y_integral + self.y_error, -self.MAX_INTEGRAL), self.MAX_INTEGRAL)

        # 计算PID输出
        x_lspeed = (self.STRAIGHT_KP * self.x_error + 
                     self.STRAIGHT_KI * self.x_integral + 
                     self.STRAIGHT_KD * (self.x_error - self.x_last_error))
                     
        y_lspeed = (self.STRAIGHT_KP * self.y_error + 
                     self.STRAIGHT_KI * self.y_integral + 
                     self.STRAIGHT_KD * (self.y_error - self.y_last_error))
                     
        yaw_aspeed = self.YAW_KP * self.yaw_error + self.YAW_KD * (self.yaw_error - self.yaw_last_error)

        # 更新上一个误差
        self.x_last_error = self.x_error
        self.y_last_error = self.y_error
        self.yaw_last_error = self.yaw_error

        # 输出限幅
        x_lspeed = max(min(x_lspeed, self.MAX_LSPEED), -self.MAX_LSPEED)
        y_lspeed = max(min(y_lspeed, self.MAX_LSPEED), -self.MAX_LSPEED)
        yaw_aspeed = max(min(yaw_aspeed, self.MAX_ASPEED), -self.MAX_ASPEED)
        

        

        


        # 发布速度
        self.vel_msg.linear.x = x_lspeed * 0.01
        self.vel_msg.linear.y = y_lspeed * 0.01
        self.vel_msg.angular.z = yaw_aspeed

        self.pub.publish(self.vel_msg)

    def run(self):
        # """主循环，处理TF变换。"""
        rate = rospy.Rate(10)
        rospy.Subscriber('/odom_out', Odometry, callback_actspeed)
        while not rospy.is_shutdown():
            try:
                transform = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0))
                self.callback(transform)
            except tf2_ros.TransformException as e:
                rospy.logwarn("Transform not available: %s", e)
            rate.sleep()

if __name__ == '__main__':
    try:
        controller = RobotController()
        controller.run()
    except rospy.ROSInterruptException:
        pass
